package de.ifgi;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.TreeSet;

import Jama.Matrix;
import android.app.IntentService;
import android.content.Intent;
import android.util.Log;

public class Localization extends IntentService {
	
	private static final String tag = "OIL";
	private long startTime;
	private long stopTime;
	private NodesList nodesList;
	
	public Localization() {
		super("de.ifgi.Localization");
	}

	@Override
	protected void onHandleIntent(Intent intent) {
		nodesList = NodesList.getNodesList();
//		Log.d(tag, "Size="+nodesList.size());
		for (Node n : nodesList)
			Log.d(tag, n.getBtName());
		
		Point userPos = localizeMe();
//		Point userPos = localizeMeSlow();
//		Point userPos = leastSquareMe();
		
//		Point userPos = new Point();
//		String strAction = intent.getStringExtra("action");
//		Log.d(tag, strAction);
//		if (strAction.equalsIgnoreCase("localizeMe1"))
//			userPos = localizeMe();
//		else if (strAction.equalsIgnoreCase("localizeMe2"))
//			userPos = localizeMeSlow();
//		else if (strAction.equalsIgnoreCase("leastSquare"))
//			userPos = leastSquareMe();
		
		Log.d(tag, userPos.getX()+","+userPos.getY());
		
		Intent sendNodesInt = new Intent("de.ifgi.oil.action.NODES_TO_RECEIVER");
		sendNodesInt.putExtra("userPos", userPos);
		sendBroadcast(sendNodesInt);
	}
	
	public Point localizeMe() {		
		double scale;
		double n = 0; // faktor pengurang untuk scale
		
		Point finalPt = new Point();
        		
		Node node1 = nodesList.get(0);
		Node node2 = nodesList.get(1);
		Node node3 = nodesList.get(2);
		
        double r1, r2, r3;
		
		double x1 = node1.getEstPosition().getX();
		double y1 = node1.getEstPosition().getY();
		double x2 = node2.getEstPosition().getX();
		double y2 = node2.getEstPosition().getY();
		double x3 = node3.getEstPosition().getX();
		double y3 = node3.getEstPosition().getY();
		
		int counter = 1;
		
        startTime = System.currentTimeMillis();
        
		while (n < 0.5) {
//		while (!isGoodEnough) {
        	scale = 1.0 - n;
        	
        	r1 = node1.getDistanceFromRssi() * scale;
        	r2 = node2.getDistanceFromRssi() * scale;
        	r3 = node3.getDistanceFromRssi() * scale;
        	
        	Log.d(tag, "\n\nr1="+r1+", r2="+r2+", r3="+r3);
        	
        	// if rssi from a node is so strong, users might be near the node itself.
//        	if (counter == 1) {
//        		double tempx = 0;
//        		double tempy = 0;
//        		int numNodes = 0;
//        		boolean isUserNearANode = false;
//        		// kalo deket sama >1 node, cari mean-nya.
//        		if (r1<=1) {
//        			tempx += x1;
//        			tempy += y1;
//        			numNodes++;
//        			isUserNearANode = true;
//        		}
//        		if (r2<=1) {
//        			tempx += x2;
//        			tempy += y2;
//        			numNodes++;
//        			isUserNearANode = true;
//        		}
//        		if (r3<=1) {
//        			tempx += x3;
//        			tempy += y3;
//        			numNodes++;
//        			isUserNearANode = true;
//	        	}
//        		if (isUserNearANode) {
//        			finalPt.setX(tempx/numNodes);
//        			finalPt.setY(tempy/numNodes);
//	        		Log.d(tag, "User location is roughly near the nodes");
//	        		break;
//        		}
//        	}
        	
        	counter++;
        	
        	if (r1<=0 || r2<=0 || r3<=0) {
        		Log.d(tag, "One of the radii<=0");
        		break;
        	}
        	
        	SignalCircles sc12 = new SignalCircles(node1.getEstPosition(), r1, node2.getEstPosition(), r2);
			SignalCircles sc13 = new SignalCircles(node1.getEstPosition(), r1, node3.getEstPosition(), r3);
			SignalCircles sc23 = new SignalCircles(node2.getEstPosition(), r2, node3.getEstPosition(), r3);
			
//			Log.d(tag, sc12.intersect()+","+sc13.intersect()+","+sc23.intersect());
        	
        	if (sc12.intersect() == true && sc13.intersect() == true && sc23.intersect() == true) {
        		List<Point> intersectionPts = new ArrayList<Point>();
        		
        		intersectionPts.addAll(sc12.getIntersectionPoints());
        		intersectionPts.addAll(sc13.getIntersectionPoints());
        		intersectionPts.addAll(sc23.getIntersectionPoints());
        		
        		// get three points with the smallest distance between them
				List<Point> clusterPts = getClusterPoints(intersectionPts);
//				Log.d(tag, "numClusterPts="+clusterPts.size());
				
		        double sumDist = 0;
		        double bestSumDist = Double.MAX_VALUE;
				
				// get sum distances --> Pt0-Pt1, Pt1-Pt2, Pt2-Pt0
				sumDist += clusterPts.get(0).distance(clusterPts.get(1));
				sumDist += clusterPts.get(1).distance(clusterPts.get(2));
				sumDist += clusterPts.get(2).distance(clusterPts.get(0));
				
				double deltax = 0.0;
				double deltay = 0.0;
				
        		// TODO: kalo sumDist > bestSumDist, untuk testData #5, loop kedua, listPosnya kosong
//        		if (sumDist >= bestSumDist) {
//        			isGoodEnough = true;
//        		} else {
				
				if (sumDist < bestSumDist) {
        			bestSumDist = sumDist;
        			// get average (x,y)
        			for (Point p : clusterPts) {
        				deltax += p.getX();
        				deltay += p.getY();
        			}
        			finalPt.setX(deltax/clusterPts.size());
        			finalPt.setY(deltay/clusterPts.size());
        			Log.d(tag, "\nFinal x,y=" + finalPt.getX() + "," + finalPt.getY());
        		}
				
        	} else {
        		Log.d(tag, "Not intersect");
        		break;
        	}
        	n += 0.05;
        }
		
		stopTime = System.currentTimeMillis();
        Log.d(tag, "\nTime taken=" + (stopTime-startTime) + " ms");
        return finalPt;
	}
	
	public Point localizeMeSlow() {
		double scale;
		
		Point finalPt = new Point();
		
		Node node1 = nodesList.get(0);
		Node node2 = nodesList.get(1);
		Node node3 = nodesList.get(2);
		
        double r1, r2, r3;
		
		double x1 = node1.getEstPosition().getX();
		double y1 = node1.getEstPosition().getY();
		double x2 = node2.getEstPosition().getX();
		double y2 = node2.getEstPosition().getY();
		double x3 = node3.getEstPosition().getX();
		double y3 = node3.getEstPosition().getY();
		
		int counter = 1;
		
		startTime = System.currentTimeMillis();
		
		double n1 = 0.0;
        while (n1 < 0.5) {
			scale = 1.0 - n1;
			r1 = node1.getDistanceFromRssi() * scale;

			double n2 = 0.0;
			while (n2 < 0.5) {
				scale = 1.0 - n2;
	        	r2 = node2.getDistanceFromRssi() * scale;

				double n3 = 0.0;
				while (n3 < 0.5) {
					scale = 1.0 - n3;
		        	r3 = node3.getDistanceFromRssi() * scale;
		        	
					Log.d(tag, "\nr1="+r1+", r2="+r2+", r3="+r3);
					
					// if rssi from a node is so strong, users might be near the node itself.
//		        	if (counter == 1) {
//		        		double tempx = 0;
//		        		double tempy = 0;
//		        		int numNodes = 0;
//		        		boolean isUserNearANode = false;
//		        		// kalo deket sama >1 node, cari mean-nya.
//		        		if (r1<=1) {
//		        			tempx += x1;
//		        			tempy += y1;
//		        			numNodes++;
//		        			isUserNearANode = true;
//		        		}
//		        		if (r2<=1) {
//		        			tempx += x2;
//		        			tempy += y2;
//		        			numNodes++;
//		        			isUserNearANode = true;
//		        		}
//		        		if (r3<=1) {
//		        			tempx += x3;
//		        			tempy += y3;
//		        			numNodes++;
//		        			isUserNearANode = true;
//			        	}
//		        		if (isUserNearANode) {
//		        			finalPt.setX(tempx/numNodes);
//		        			finalPt.setY(tempy/numNodes);
//			        		Log.d(tag, "User location is roughly near the nodes");
//			        		break;
//		        		}
//		        	}
		        	
		        	counter++;
		        	
		        	if (r1<=0 || r2<=0 || r3<=0) {
		        		Log.d(tag, "One of the radii<=0");
		        		break;
		        	}
		        	
		        	SignalCircles sc12 = new SignalCircles(node1.getEstPosition(), r1, node2.getEstPosition(), r2);
					SignalCircles sc13 = new SignalCircles(node1.getEstPosition(), r1, node3.getEstPosition(), r3);
					SignalCircles sc23 = new SignalCircles(node2.getEstPosition(), r2, node3.getEstPosition(), r3);
		        	
		        	if (sc12.intersect() == true && sc13.intersect() == true && sc23.intersect() == true) {
		        		ArrayList<Point> intersectionPts = new ArrayList<Point>();
		        		
		        		intersectionPts.addAll(sc12.getIntersectionPoints());
		        		intersectionPts.addAll(sc13.getIntersectionPoints());
		        		intersectionPts.addAll(sc23.getIntersectionPoints());
		        		
		        		// get three points with the smallest distance between them
						List<Point> clusterPts = getClusterPoints(intersectionPts);
						
				        double sumDist = 0.0;
				        double bestSumDist = Double.MAX_VALUE;
						
						// get sum distances --> Pt0-Pt1, Pt1-Pt2, Pt2-Pt0
						sumDist += clusterPts.get(0).distance(clusterPts.get(1));
						sumDist += clusterPts.get(1).distance(clusterPts.get(2));
						sumDist += clusterPts.get(2).distance(clusterPts.get(0));
						
						double deltax = 0.0;
						double deltay = 0.0;
						
						if (sumDist < bestSumDist) {
		        			bestSumDist = sumDist;
		        			// get average (x,y)
		        			for (Point p : clusterPts) {
		        				deltax += p.getX();
		        				deltay += p.getY();
		        			}
		        			finalPt.setX(deltax/clusterPts.size());
		        			finalPt.setY(deltay/clusterPts.size());
		        			Log.d(tag, "\nFinal x,y=" + finalPt.getX() + "," + finalPt.getY());
		        			Log.d(tag, "\n\nr1="+r1+", r2="+r2+", r3="+r3);
		        		}

		        	} else {
		        		Log.d(tag, "Not intersect");
		        		break;
		        	}
					n3 += 0.1;
				}
				n2 += 0.1;
			}
			n1 += 0.1;
		}
        
		stopTime = System.currentTimeMillis();
        Log.d(tag, "\nTime taken=" + (stopTime-startTime) + " ms");
        return finalPt;
	}
	
	public Point leastSquareMe() {
		Point finalPt = new Point();

		Node node1 = nodesList.get(0);
		Node node2 = nodesList.get(1);
		Node node3 = nodesList.get(2);
		
		double r1 = node1.getDistanceFromRssi();
        double r2 = node2.getDistanceFromRssi();
        double r3 = node3.getDistanceFromRssi();
		
		double x1 = node1.getEstPosition().getX();
		double y1 = node1.getEstPosition().getY();
		double x2 = node2.getEstPosition().getX();
		double y2 = node2.getEstPosition().getY();
		double x3 = node3.getEstPosition().getX();
		double y3 = node3.getEstPosition().getY();
		
		startTime = System.currentTimeMillis();
		
		SignalCircles sc12 = new SignalCircles(node1.getEstPosition(), r1, node2.getEstPosition(), r2);
		SignalCircles sc13 = new SignalCircles(node1.getEstPosition(), r1, node3.getEstPosition(), r3);
		SignalCircles sc23 = new SignalCircles(node2.getEstPosition(), r2, node3.getEstPosition(), r3);
		
		if (sc12.intersect() == true && sc13.intersect() == true && sc23.intersect() == true) {
    		List<Point> intersectionPts = new ArrayList<Point>();
    		
    		intersectionPts.addAll(sc12.getIntersectionPoints());
    		intersectionPts.addAll(sc13.getIntersectionPoints());
    		intersectionPts.addAll(sc23.getIntersectionPoints());
    		
    		// get three points with the smallest distance between them
			List<Point> clusterPts = getClusterPoints(intersectionPts);
			
			// average cluster points
			double deltax = 0.0;
			double deltay = 0.0;
			
			for (Point p : clusterPts) {
				deltax += p.getX();
				deltay += p.getY();
			}
			
			double estx = deltax/clusterPts.size();
			double esty = deltay/clusterPts.size();
			Log.d(tag, "\nInit x,y=" + estx + "," + esty);
			
			for (int i=0; i<5; i++) {
				double deltax1 = estx-x1;
				double deltay1 = esty-y1;
				double deltax2 = estx-x2;
				double deltay2 = esty-y2;
				double deltax3 = estx-x3;
				double deltay3 = esty-y3;

				double sqrt1 = Math.sqrt(Math.pow((deltax1), 2) + Math.pow((deltay1), 2));
				double sqrt2 = Math.sqrt(Math.pow((deltax2), 2) + Math.pow((deltay2), 2));
				double sqrt3 = Math.sqrt(Math.pow((deltax3), 2) + Math.pow((deltay3), 2));

				double deltar1 = r1 - sqrt1;
				double deltar2 = r2 - sqrt2;
				double deltar3 = r3 - sqrt3;
				double[] deltar = {deltar1, deltar2, deltar3};
				Matrix mDeltar = new Matrix(deltar, 3);

				double[][] arrJ = {{deltax1/sqrt1, deltay1/sqrt1}, {deltax2/sqrt2, deltay2/sqrt2}, {deltax3/sqrt3, deltay3/sqrt3}};
				Matrix mJ = new Matrix(arrJ);

				Matrix mTrp = mJ.transpose();
				Matrix mRes = (mTrp.times(mJ)).inverse().times(mTrp).times(mDeltar);
				double[] arrRes = mRes.getColumnPackedCopy();

				estx -= arrRes[0];
				esty -= arrRes[1];
				Log.d(tag, "estx, esty="+estx+","+esty);
			}
			
			finalPt.setX(estx);
			finalPt.setY(esty);
			
		} else {
			Log.d(tag, "Not intersect");
		}
		
		stopTime = System.currentTimeMillis();
        Log.d(tag, "\nTime taken=" + (stopTime-startTime) + " ms");
		return finalPt;
	}
	
	/**
	 * Returns three points with the smallest distance between them.
	 * @param listPoints List of six intersection points.
	 * @return Three points with the smallest distance.
	 */
    public List<Point> getClusterPoints(List<Point> intersectionPts) {
    	int counter = 1;
    	Point p1, p2;
    	double d;
    	Map<List<Point>, Double> hmDistOrd = new HashMap<List<Point>, Double>();
    	
    	for (int i=0; i<intersectionPts.size(); i++) {
			p1 = intersectionPts.get(i);
    		for (int j=0; j<intersectionPts.size(); j++) {
    			p2 = intersectionPts.get(j);
    			if (!p1.equals(p2)) {
    		    	Point[] keyPts = new Point[2];
    				keyPts[0] = p1;
    				keyPts[1] = p2;
    				d = p1.distance(p2);
//    				Log.d(tag, "d" + counter + "=" + d);
    				hmDistOrd.put(Arrays.asList(keyPts), d);
    				counter=counter+1;
    			}
    		}
    	}
//    	Log.d(tag, "numDistOrd="+hmDistOrd.size());
    	
    	// sort the distances
    	List<List<Point>> mapKeys = new ArrayList<List<Point>>(hmDistOrd.keySet());
//    	Log.d(tag, "numKeys="+mapKeys.size());
    	List<Double> mapValues = new ArrayList<Double>(hmDistOrd.values());
//    	Log.d(tag, "numValues="+mapValues.size());
    	
    	Map<List<Point>, Double> hmSortOrd = new LinkedHashMap<List<Point>, Double>();
    	
    	TreeSet<Double> tsSortOrd = new TreeSet<Double>(mapValues);
    	Object[] sortedArray = tsSortOrd.toArray();
    	int size = sortedArray.length;
//    	Log.d(tag, "sortedArray="+sortedArray.length);
    	
    	// descending
    	for (int i=0; i<size; i++) {
    		hmSortOrd.put(mapKeys.get(mapValues.indexOf(sortedArray[i])), (Double)sortedArray[i]);
//    		Log.d(tag, mapKeys.get(mapValues.indexOf(sortedArray[i])).toString() + "=" + sortedArray[i].toString());
    	}
    	
//    	Log.d(tag, "numSortOrd="+hmSortOrd.size());

    	// get the top three entries
    	int inc = 1;
    	List<Point> tmpPts = new ArrayList<Point>();
    	List<Point> clusterPts = new ArrayList<Point>();
    	
    	outerloop:
    		for (Map.Entry<List<Point>, Double> e : hmSortOrd.entrySet()) {
    			// if there are already 3 different points, stop
    			tmpPts = e.getKey();

    			for (Point p : tmpPts) {
    				if (inc == 4)
    					break outerloop;

    				if (!clusterPts.contains(p)) {
    					clusterPts.add(p);
//    					Log.d(tag, "\nResult " + inc + ":" + p.getX() + "," + p.getY());
    					inc++;
    				}
    			}
    		}

    	return clusterPts;
    }

}
